COMMENT ⊗   VALID 00007 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002		TITLE UPDATE
C00003 00003	THESE ROUTINES TOGETHER WITH SAITRG[SYS,HE] OCCUPY 1243 OCTAL WORDS
C00004 00004		BEGIN UPDATE
C00022 00005	MUL:	MOVEI J,2
C00023 00006	TH:	BLOCK 6
C00024 00007		BEGIN TRANSMULT
C00025 ENDMK
C⊗;
	TITLE UPDATE

	↓BLUE←←1	;assemble for blue arm
IFN BLUE,<
	↓LH←←1		;left hand arm (blue)
	↓RH←←0		;right hand arm (yellow)
>
IFE BLUE,<
	↓LH←←0		;left hand arm (blue)
	↓RH←←1		;right hand arm (yellow)
>
;THESE ROUTINES TOGETHER WITH SAITRG[SYS,HE] OCCUPY 1243 OCTAL WORDS

	INTERNAL UPDATE
	EXTERNAL ACOS,SQRT,ATAN2,ASIN,ACOS,SIND,COSD

	↓AC←1
	↓P←17

IFE BLUE,<
	S2V←←6.05
	S6V←←9.38
>
IFN BLUE,<
	S2V←←-6.05
	S6V←←9.28125
>
	S22V←←36.6025

↓RAD:	206712273406
IFN BLUE,<
↓SHOLDER:29.53125
	50.805
	20.24
>
IFE BLUE,<
↓SHOLDER:29.5
	8.375
	16.24
>
	BEGIN UPDATE
;UPDATE(T,J)
;GIVEN JOINT ANGLES J COMPUTES THE TRANSFORMATION T
;TIME APPROX 6 M. SEC

	A1←←2
	A2←←3
	A3←←4
	R1←←5
	R2←←6
	R3←←7
	I←←10
	J←←11
	K←←12

↑UPDATE:
	MOVEM 16,HOLD+16
	HRRZI 16,HOLD
	BLT 16,HOLD+15
	HRL AC,-1(P)		;TH
	HRRI AC,TH
	BLT AC,TH+5
	SETZM T			;SET T TO UNIT MATRIX
	MOVE AC,[XWD T,T+1]
	BLT AC,T+13
	MOVSI AC,(1.0)
	MOVEM AC,T
	MOVEM AC,T+5
	MOVEM AC,T+12
	SETZM LINK
MATMUL:				;MATMUL	T(K)←T(K-1)*A(K)
	MOVE AC,[XWD T,TN]
	BLT AC,TN+13		;MOVE T INTO TN
	MOVE K,LINK
	CAIN K,2
	JRST[	MOVE AC,SI3
		MOVEM AC,SI
		MOVE AC,CO3
		MOVEM AC,CO
		MOVE AC,TH+2
		MOVEM AC,S+2
		JRST IDSU]
	PUSH P,TH(K)
	PUSHJ P,SIND
	MOVEM AC,SI
	MOVE K,LINK
	PUSH P,TH(K)
	PUSHJ P,COSD
	MOVEM AC,CO
IDSU:	MOVE K,LINK
;FIRST ROW
	MOVE A1,CO
	MOVE A2,SI
	SETZ A3,
	MOVEI I,0
	PUSHJ P,MUL
;SECOND ROW
	MOVN A1,CA(K)
	FMPR A1,SI
	MOVE A2,CA(K)
	FMPR A2,CO
	MOVE A3,SA(K)
	MOVEI I,1
	PUSHJ P,MUL
;THIRD ROW
	MOVE A1,SA(K)
	FMPR A1,SI
	MOVN A2,SA(K)
	FMPR A2,CO
	MOVE A3,CA(K)
	MOVEI I,2
	PUSHJ P,MUL
;FOURTH ROW
	SETZ A1,
	SETZ A2,
	MOVE A3,S(K)
	MOVEI I,3
	PUSHJ P,MUL
	MOVE AC,TN+3		;OLD X
	FADRM AC,T+3		;NEW X
	MOVE AC,TN+7		;OLD Y
	FADRM AC,T+7		;NEW Y
	MOVE AC,TN+13		;OLD Z
	FADRM AC,T+13		;NEW Z
	AOS K,LINK
	CAIGE K,6
	JRST MATMUL
	HRRZ K,-2(P)
	MOVEI AC,13(K)
	HRLI K,T
	BLT K,(AC)
	HRRZ K,-2(P)
	MOVE AC,SHOLDER
	FADRM AC,3(K)		;X
	MOVE AC,SHOLDER+1
	FADRM AC,7(K)		;Y
	MOVE AC,SHOLDER+2
	FADRM AC,13(K)		;Z
	HRLZI 16,HOLD
	BLT 16,15
	MOVE 16,HOLD+16
	SUB P,[3(3)]
	JRST @3(P)

MUL:	MOVEI J,2
L2:	MOVE AC,PW(J)
	BLT AC,R1+2
	FMPR R1,A1
	FMPR R2,A2
	FMPR R3,A3
	FADR R1,R2
	FADR R1,R3
	MOVEM R1,T(I)
	ADDI I,4
	SOJGE J,L2
	POPJ P,
TH:	BLOCK 6
PW:	XWD TN+10,R1
	XWD TN+4,R1
	XWD TN,R1
LINK:	0
SI:	0
CO:	0
T:	BLOCK 14
TN:	BLOCK 14
SI3:	-1.0
CO3:	0.0
S:	0.0
	S2V
	0.0
	0.0
	0.0
	S6V
SA:	-1.0
	1.0
	0.0
	-1.0
	1.0
	0.0
CA:	0.0
	0.0
	1.0
	0.0
	0.0
	1.0
HOLD:	BLOCK 17
	BEND
	BEGIN TRANSMULT
	S←1
	M←2
	A1←3
	A2←4
	A3←5
	A4←6
	I←7
	J←10
	R←11
	T←14
	U←13
↑TIMES:HRL T,-1(P)
	HRRI T,MLT
	BLT T,MLT+13
	MOVEI T,MLT
	HRRZ U,-2(P)
	HRRZ R,-3(P)
	MOVEI I,3

L1:	MOVEI J,3

	MOVE A1,(U)
	MOVE A2,1(U)
	MOVE A3,2(U)
L2:	MOVE S,A1
	FMPR S,(T)
	MOVE M,A2
	FMPR M,4(T)
	FADR S,M
	MOVE M,A3
	FMPR M,10(T)
	FADR S,M
	MOVEM S,(R)
	AOJ R,
	AOJ T,
	SOJG J,L2

	FMPR A1,(T)
	FMPR A2,4(T)
	FADR A1,A2
	FMPR A3,10(T)
	FADR A1,A3
	FADR A1,3(U)
	MOVEM A1,(R)
	AOJ R,
	SUBI T,3
	ADDI U,4
	SOJG I,L1
	SUB P,[XWD 4,4]
	JRST @4(P)
MLT:	BLOCK 14
	BEND

	END